/**

This file is part of MaCI/GIMnet.

MaCI/GIMnet is free software: you can redistribute it and/or modify it 
under the terms of the GNU Lesser General Public License as published 
by the Free Software Foundation, either version 3 of the License, or 
(at your option) any later version.

MaCI/GIMnet is distributed in the hope that it will be useful, but WITHOUT 
ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or 
FITNESS FOR A PARTICULAR PURPOSE.  See the GNU Lesser General Public 
License for more details.

You should have received a copy of the GNU Lesser General Public 
License along with GIMnet. (See COPYING.LESSER) If not, see 
<http://www.gnu.org/licenses/>.

**/
#include<unistd.h>
#include <string>
#include <iostream>
#include <fstream> 
#include "CGridGServerModule.h"
#include "glutImageGUI.h"
#include "CImageClient.h"
#include "../stereoCR/StereoRoutines.h"

//#define SHOW_WAVES
//#define LOAD_STATIC_IMAGES
//#define USE_CAMERA_IMAGES
#define TEST_MODE

std::string paramHubAdress="asrobo.hut.fi";
int paramHubPort=50002;
std::string paramOwnName="grid_server";
std::string paramImageServerName="fw_server";

int paramDebugLevel = 10;

typedef float grid_t;

void  printUsage(){
	fprintf(stderr,"Grid server\n");
	fprintf(stderr,"Options:\n");
	fprintf(stderr,"-o string : set the own hub name\n");
	fprintf(stderr,"-d int : Set debug level\n");
	fprintf(stderr,"-p int : Set hub port\n");
	fprintf(stderr,"-h string : Hub name\n");
	fprintf(stderr,"-s string : Image server name\n");
	fprintf(stderr,"-? Usage\n");
	fprintf(stderr,"---------------------------------------\n");
	
}
void parse(int argc,char **argv){
	bool paramParseLoop = true;
	int option_char;
	/*** Set options ***/
	while (paramParseLoop) {
		option_char = getopt(argc, argv, "o:d:h:p:s:");
		switch (option_char) {
			case 'o':
				if (optarg != NULL)
					paramOwnName = optarg;
				fprintf(stderr,"Setting own name to %s\n",paramOwnName.c_str());
				break;
			case 'd':
				if (optarg != NULL)
					paramDebugLevel = atoi (optarg);
				fprintf(stderr,"Debug level=%d\n",paramDebugLevel);
				break;
			case 'h':
				if (optarg != NULL)
					paramHubAdress = optarg;
			  
				fprintf(stderr,"Hub adress set to %s\n",paramHubAdress.c_str());
				break;
			case 's':
				if (optarg != NULL)
					paramImageServerName = optarg;
			  
				fprintf(stderr,"Image server name set to %s\n",paramImageServerName.c_str());
				break;
			case 'p':
				if (optarg != NULL)
					paramHubPort = atoi (optarg);
				fprintf(stderr,"Hub port set to %d\n",paramHubPort);
				break;
			case '?':
				printUsage();
				exit(1);
				break;
			case -1:
				paramParseLoop = false;
				break;
		}
	}
}


char * loadRawImage(const char * filename)
{
    std::ifstream::pos_type size;
    char memblock[100]={0};
    char chrBuf;
    int i, j, counter, k, pos;
    int width=0, height=0;
    char terminalSequence[3]={0,0,0};
    int terminalSequenceLength=0;
    bool isComment = false;
    char * YUV = NULL;
 
    std::ifstream file(filename, std::ios::in|std::ios::binary|std::ios::ate);
    
    if (file.is_open())
    {
        size = file.tellg();
        
        file.seekg(0, std::ios::beg);
 
        for (i=0, counter=0, j=0, pos=0; counter<3; i++)
        {
            file.read(&chrBuf, 1);
            pos++;

            if (terminalSequenceLength == 1 && 
                terminalSequence[0] != chrBuf &&
                (chrBuf == 10 || chrBuf == 13))
            {
                terminalSequence[1] = chrBuf;
                terminalSequenceLength = 2;
            }
            
            if (terminalSequenceLength == 0 && 
                (chrBuf == 10 || chrBuf == 13))
            {
                terminalSequence[0] = chrBuf;
                terminalSequenceLength = 1;
            }
            
            if (chrBuf == terminalSequence[0])
            {
                if (counter == 0 && !isComment)
                {
                    if (memblock[0] != 'Y' || memblock[1] != '7')
                    {
                        file.close();
                        printf("Not Y7 type\n");
                        return NULL;
                    }
                }
                if (counter == 1 && !isComment)
                {
                    for (k=0;
                         memblock[k] != ' ' && memblock[k] != '\0';
                         k++);
                    if (memblock[k] == ' ')
                    {
                        memblock[k] = '\0';
                        width = (int) atof(memblock);
                        height = (int) atof(&memblock[k+1]);
                    }
                }
                counter++;
                j = 0;
                isComment = false;
            }
            if (chrBuf == '#')
            {
                counter--;
                isComment = true;
            }
            if (chrBuf >= ' ')
            {
                memblock[j] = chrBuf;
                memblock[j+1] = '\0';
                if (++j >= 100)
                {
                    file.close();
                    printf("memblock buffer size (100 bytes) exceeded\n");
                    return NULL;
                }
            }
        }
        
        if (terminalSequenceLength == 2 && chrBuf == terminalSequence[0])
        {
            file.read(&chrBuf, 1);

            if (chrBuf == terminalSequence[1])
            {
                pos++;
            }
            else
            {
                printf("Unknown character in file\n");
                return NULL;
            }
        }
        
        if (width <= 0 || height <= 0)
        {
            file.close();
            printf("Width or height = 0\n");
            return NULL;
        }
        
        YUV = new char[width*height*2];
        file.read(YUV, width*height*2);
        file.close();
    }
    
    return YUV;
}

void show_uchar(int imnum, unsigned char * buf, int width, int height)
{
	unsigned char * rgb = new unsigned char[640*480*3];

	for(int j=0,k=0,l=0;j<height;j++)
	{
		for(int i=0;i<width;i++,k++,l+=3)
		{
			rgb[l] = (unsigned char) (buf[k]);
			rgb[l+1] = (unsigned char) (buf[k]);
			rgb[l+2] = (unsigned char) (buf[k]);
		}
	}
  displayImage(imnum, rgb,height,width);
}


void show_dispmap(int imnum, short * buf, int width, int height)
{
	unsigned char * rgb = new unsigned char[640*480*3];
	float scalefactor = 255.0/8500.0;
	float min=1e10, max=-1e10;
	float num;

	for(int j=0,k=0,l=0;j<height;j++)
	{
		for(int i=0;i<width;i++,k++,l+=3)
		{
		  num = (unsigned char) (buf[k]*scalefactor);
		  if (num<min)
		    min = num;
		  if (num>max)
		    max = num;
		  
			rgb[l] = (unsigned char) (buf[k]*scalefactor);
			rgb[l+1] = (unsigned char) (buf[k]*scalefactor);
			rgb[l+2] = (unsigned char) (buf[k]*scalefactor);
		}
	}
  displayImage(imnum, rgb,height,width);
  printf("min=%.3f max=%.3f\n", min, max);
}


void show_float(int imnum, float * buf, int width, int height)
{
	unsigned char * rgb = new unsigned char[640*480*3];
	float min=1e10, max=-1e10;
	float num;

	for(int j=0,k=0,l=0;j<height;j++)
	{
		for(int i=0;i<width;i++,k++)
		{
		  num = buf[k];
		  if (num<min)
		    min = num;
		  if (num>max)
		    max = num;
		}
	}
	
	for(int j=0,k=0,l=0;j<height;j++)
	{
		for(int i=0;i<width;i++,k++,l+=3)
		{
			rgb[l] = (unsigned char) ((buf[k]-min)/(max-min));
			rgb[l+1] = (unsigned char) ((buf[k]-min)/(max-min));
			rgb[l+2] = (unsigned char) 0;
		}
	}
  displayImage(imnum, rgb,height,width);
}



int main(int argc,char **argv){
	int Nx,Ny;
	gimi::GIMI gim(1);
	TGIMI_PROTOCOL_GRID_HEADER head;
  TBitmap img0;
  TBitmap img1;
	int gridsize=100;
	
  float minX = -2.5;
  float maxX = 2.5;
  float minY = 0;
  float maxY = 5;
  float minZ = -0.5;
  float maxZ = 50;
  float unknownValue = -2.5;
  float cellsize;
  
	parse(argc,argv);
	/*
	int32_t size;    ///< The size of grid data in bytes
	int32_t Nx;      ///< The number of elements in x
	int32_t Ny;      ///< The number of elements in y
	float resolution;///< size of one cell in meters			
	float x;         ///< The global position of the grid origin 
	float y;         ///< The global position of the grid origin 
	float a;         ///< The global position of the grid origin 
	int32_t orig_x;  ///< The placement of the grid origin within the grid
	int32_t orig_y;  ///< The placement of the grid origin within the grid 
	unsigned char format;	///< The format of the grid data (GRID_FORMAT_TYPE_XXX)
	*/
	head.size = gridsize*gridsize*sizeof(grid_t);
	head.Nx = gridsize;
	head.Ny = gridsize;
	head.resolution = (maxX-minX)/gridsize;
	head.x = 50;
	head.y = 50;
	head.orig_x = gridsize/2;
  head.orig_y = gridsize/2;
	head.format = 1;	
	
  /// Initialize the debugging module
	debugInit();
	debugSetGlobalDebugLvl(paramDebugLevel);
	debugSetMode(DEBUG_MODE_TIMESTAMP_HH_MM_SS_MS);
  
	gim.connectToHub(paramHubAdress.c_str(), paramHubPort, paramOwnName.c_str());
	CGridGServerModule server(&gim);
	server.setHeader(head);

  CImageClient cli; ///< Image client interface
    
	if(!cli.initialize(g,paramImageServerName,2,CIC_bitmap, 5000)){
			dPrint(1,"Image client initialization failed - aborting");
			exit(1);
	}
	
#ifdef TEST_MODE
	addImageWindow(640, 480);
	addImageWindow(320, 240);
	glutImageGUI_Start();
#endif

	float v;
	float phii=0;
	short * dispmap = new short[640*480];
	grid_t *data = new grid_t[gridsize*gridsize];

    char * leftOutput = new char[640*480];
    char * rightOutput = new char[640*480];
    gim::StereoRoutines gsr;
    std::string dirname = "./";
    std::string num = "1234";
    
    bool everythingOK;

    while(1)
    {
        everythingOK = true;
#ifdef SHOW_WAVES
        for(int j=0;j<gridsize;j++)
        {
            v = 0.5*sin(phii+(float)j/10.0);
            for(int i=0;i<gridsize;i++)
            {
                data[j*Nx+i] = v;
            }
        }
        phii+=0.2;
#endif

#ifdef LOAD_STATIC_IMAGES
        char * leftImage = loadRawImage((dirname + "0_kuva" + num + ".ppm").c_str());
        char * rightImage = loadRawImage((dirname + "1_kuva" + num + ".ppm").c_str());
        
        if (leftImage != NULL && rightImage != NULL)
        {
            gsr.fastRectifyImages(
                leftImage, rightImage,
                leftOutput, rightOutput,
                640, 480,
                0);

            delete[] leftImage;
            delete[] rightImage;
                    		
            gim::calculateHeightMap(
                leftOutput, rightOutput,
                640, 480,
                data,
                minX, maxX, gridsize,
                minY, maxY, gridsize,
                minZ, maxZ, unknownValue,
                cellsize, 50,
                dispmap);
//        		show_dispmap(0,dispmap,640,480);
        }
        
        exit(0);
#endif

#ifdef USE_CAMERA_IMAGES
    		if(!cli.getImageCopy(&img0,0))
    		{
    				dPrint(1,"Failed to read stream 0");
    				everythingOK=false;
		    }
    		if(!cli.getImageCopy(&img1,1))
    		{
    				dPrint(1,"Failed to read stream 1");
    				everythingOK=false;
    		}
    		if (everythingOK)
            gim::calculateHeightMap(
                (char*)img0.data, (char*)img1.data,
                640, 480,
                data,
                minX, maxX, gridsize,
                minY, maxY, gridsize,
                minZ, maxZ, unknownValue,
                cellsize, 50,
                dispmap);
    		
#endif

#ifdef TEST_MODE
    		if(!cli.getImageCopy(&img0,0))
    		{
    				dPrint(1,"Failed to read stream 0");
    				everythingOK=false;
		    }
    		if(!cli.getImageCopy(&img1,1))
    		{
    				dPrint(1,"Failed to read stream 1");
    				everythingOK=false;
    		}
    		
        for(int j=0;j<gridsize;j++)
        {
            v = 0.5*sin(phii+(float)j/10.0);
            for(int i=0;i<gridsize;i++)
            {
                data[j*Nx+i] = v;
            }
        }
        phii+=0.2;
        
        show_uchar(0, (unsigned char *) img0.data(), 640, 480);
        show_uchar(1, (unsigned char *) img1.data(), 640, 480);
#endif

	      if (everythingOK)
	      {
            if(server.sendGrid((unsigned char *) data))
            {
                fprintf(stderr,"Succesfully sent data!\n");
            }
        }
	 
	      usleep(1000*1000);
    }

  	delete[] data;
  	delete[] dispmap;
  	delete[] leftOutput;
  	delete[] rightOutput;
  	
    return 0;
}
